#include "Triangle.h"
#include "TriangleMesh.h"
#include "Ray.h"
#include <iostream>
#include <xmmintrin.h>
using namespace std;


Triangle::Triangle(TriangleMesh * m, unsigned int i) :
    m_mesh(m), m_index(i)
{

}


Triangle::~Triangle()
{

}


void
Triangle::renderGL()
{
    TriangleMesh::TupleI3 ti3 = m_mesh->vIndices()[m_index];
    const Vector3 & v0 = m_mesh->vertices()[ti3.x]; //vertex a of triangle
    const Vector3 & v1 = m_mesh->vertices()[ti3.y]; //vertex b of triangle
    const Vector3 & v2 = m_mesh->vertices()[ti3.z]; //vertex c of triangle

    glBegin(GL_TRIANGLES);
        glVertex3f(v0.x, v0.y, v0.z);
        glVertex3f(v1.x, v1.y, v1.z);
        glVertex3f(v2.x, v2.y, v2.z);
    glEnd();
}


/*
bool
Triangle::intersect(HitInfo& result, const Ray& r,float tMin, float tMax)
{
	TriangleMesh::TupleI3 ti3 = m_mesh->vIndices()[m_index];
	const Vector3 & v0 = m_mesh->vertices()[ti3.x]; //vertex a of triangle
	const Vector3 & v1 = m_mesh->vertices()[ti3.y]; //vertex b of triangle
	const Vector3 & v2 = m_mesh->vertices()[ti3.z]; //vertex c of triangle

	Vector3 BmA = v1-v0;
	Vector3 CmA = v2-v0;
	Vector3 N = cross(CmA,BmA);
	N = N.normalized();

	float t = - dot(r.o-v0, N)/dot(r.d, N);

	if(t < tMin || t>tMax )
		return false;

	int i,j;
	if(fabs(N[0])>fabs(N[1])&&fabs(N[0])>fabs(N[2]))
	{
		i = 1;
		j = 2;
	}
	else
	if(fabs(N[1])>fabs(N[0])&&fabs(N[1])>fabs(N[2]))
	{
		i = 0;
		j = 2;
	}
	else
	{
		i = 0;
		j = 1;
	}

	Vector3 tmpA = (r.o+t*r.d-v0);

	float beta  = (tmpA[i]*CmA[j] - tmpA[j]*CmA[i])/(BmA[i]*CmA[j] - BmA[j]*CmA[i]);
	float gamma = (tmpA[i]*BmA[j] - tmpA[j]*BmA[i])/(CmA[i]*BmA[j] - CmA[j]*BmA[i]);

	if(beta>=0 && gamma>=0 && beta+gamma < 1)
	{
		ti3 = m_mesh->nIndices()[m_index];
		const Vector3 & n0 = m_mesh->normals()[ti3.x]; //vertex a of triangle
		const Vector3 & n1 = m_mesh->normals()[ti3.y]; //vertex b of triangle
		const Vector3 & n2 = m_mesh->normals()[ti3.z]; //vertex c of triangle
		
		result.material = m_material;
		result.t = t;
		result.P = r.o + t*r.d;
		result.N = (1-beta-gamma)*n0 + beta*n1 + gamma*n2;
		result.N = result.N.normalized();

		return true;
	}

	return false;
}
*/
bool
Triangle::intersect(HitInfo& result, const Ray& r,float tMin, float tMax)
{
	TriangleMesh::TupleI3 ti3 = m_mesh->vIndices()[m_index];
	const Vector3 & v0 = m_mesh->vertices()[ti3.x]; //vertex a of triangle
	const Vector3 & v1 = m_mesh->vertices()[ti3.y]; //vertex b of triangle
	const Vector3 & v2 = m_mesh->vertices()[ti3.z]; //vertex c of triangle

	Vector3 U1 = v1-v0;
	Vector3 U2 = v2-v1;
	Vector3 U3 = v0-v2;
	
	Vector3 V1 = cross(v1,v0);
	Vector3 V2 = cross(v2,v1);
	Vector3 V3 = cross(v0,v2);
	
	Vector3 UR = r.d;
	Vector3 VR = cross(r.d,r.o);
	
/*
	float tmp0[4], tmp1[4], tmp2[4], tmp3[4], tmp4[4], tmp5[4], tmp6[4], tmp7[4];
	
	//for(int i=0;i<4;i++)
	{
		//tmp6[i] = tmp7[i] = 0;
	}
	
	tmp0[0] = v1[1];
	tmp0[1] = v2[1];
	tmp0[2] = v0[1];
	tmp0[3] = r.d[1];
	
	tmp1[0] = v0[2];
	tmp1[1] = v1[2];
	tmp1[2] = v2[2];
	tmp1[3] = r.o[2];
	
	tmp2[0] = v1[2];
	tmp2[1] = v2[2];
	tmp2[2] = v0[2];
	tmp2[3] = r.d[2];
	
	tmp3[0] = v0[1];
	tmp3[1] = v1[1];
	tmp3[2] = v2[1];
	tmp3[3] = r.o[1];
	
	tmp4[0] = v0[0];
	tmp4[1] = v1[0];
	tmp4[2] = v2[0];
	tmp4[3] = r.o[0];
	
	tmp5[0] = v1[0];
	tmp5[1] = v2[0];
	tmp5[2] = v0[0];
	tmp5[3] = r.d[0];

	__m128 t0, t1, t2, t3, t4, t5, t6, t7;
	
	float res0[4], res1[4], res2[4];
	//float * res0 = new float[4];
	//float * res1 = new float[4];
	//float * res2 = new float[4];
	
	t0 = _mm_loadu_ps(tmp0);
	t1 = _mm_loadu_ps(tmp1);
	t2 = _mm_loadu_ps(tmp2);
	t3 = _mm_loadu_ps(tmp3);
	t4 = _mm_loadu_ps(tmp4);
	t5 = _mm_loadu_ps(tmp5);
	t6 = _mm_loadu_ps(tmp6);
	t7 = _mm_loadu_ps(tmp7);
	
	/*
	t6 = _mm_mul_ps(t0,t1);
	t7 = _mm_mul_ps(t2,t3);
	t6 = _mm_sub_ps(t6,t7);
	_mm_storeu_ps(res0, t6);
	
	t6 = _mm_mul_ps(t2,t4);
	t7 = _mm_mul_ps(t5,t1);
	t1 = _mm_sub_ps(t6,t7);
	
	
	t6 = _mm_mul_ps(t5,t3);
	t7 = _mm_mul_ps(t0,t4);
	t2 = _mm_sub_ps(t6,t7);
	
	_mm_storeu_ps(res1,t1);
	_mm_storeu_ps(res2,t2);
	*/
	
	
/*	
	
	
	
	

	t6 = _mm_mul_ps(t0,t1);
	t0 = _mm_mul_ps(t0,t4);
	t7 = _mm_mul_ps(t2,t3);
	t2 = _mm_mul_ps(t2,t4);
	t1 = _mm_mul_ps(t1,t5);
	t5 = _mm_mul_ps(t5,t3);
	
	t6 = _mm_sub_ps(t6,t7);
	t2 = _mm_sub_ps(t2,t1);
	t5 = _mm_sub_ps(t5,t0);
	
	_mm_storeu_ps(res0,t6);
	_mm_storeu_ps(res1,t2);
	_mm_storeu_ps(res2,t5);
	
	
	
	
	
	
	V1[0] = res0[0];
	V2[0] = res0[1];
	V3[0] = res0[2];
	VR[0] = res0[3];
	
	V1[1] = res1[0];
	V2[1] = res1[1];
	V3[1] = res1[2];
	VR[1] = res1[3];
	
	V1[2] = res2[0];
	V2[2] = res2[1];
	V3[2] = res2[2];
	VR[2] = res2[3];
*/	
	//return false;
	Vector3 bar;
	
	bar[2] = dot(U1,VR) + dot(UR, V1);
	bar[0] = dot(U2,VR) + dot(UR, V2);
	bar[1] = dot(U3,VR) + dot(UR, V3);
	
	float _bar[4], _UR[4], _VR[4], 
				   _U1[4], _V1[4],
				   _U2[4], _V2[4],
				   _U3[4], _V3[4],
				   
				   
                                   AX[4], AY[4], AZ[4],
                                   BX[4], BY[4], BZ[4],
                                   CX[4], CY[4], CZ[4],
                                   DX[4], DY[4], DZ[4];
				
        AX[0] = U1[0];
        AX[1] = U2[0];
        AX[2] = U3[0];
        //_AX[3] = UR[0];
	
        AY[0] = U1[1];
        AY[1] = U2[1];
        AY[2] = U3[1];

        AZ[0] = U1[2];
        AZ[1] = U2[2];
        AZ[2] = U3[2];

        BX[0] = V1[0];
        BX[1] = V2[0];
        BX[2] = V3[0];
        //_AX[3] = UR[0];

        BY[0] = V1[1];
        BY[1] = V2[1];
        BY[2] = V3[1];

        BZ[0] = V1[2];
        BZ[1] = V2[2];
        BZ[2] = V3[2];

        CX[0] = VR[0];
        CX[1] = VR[0];
        CX[2] = VR[0];
        //_AX[3] = UR[0];

        CY[0] = VR[1];
        CY[1] = VR[1];
        CY[2] = VR[1];

        CZ[0] = VR[2];
        CZ[1] = VR[2];
        CZ[2] = VR[2];

        DX[0] = UR[0];
        DX[1] = UR[0];
        DX[2] = UR[0];
        //_AX[3] = UR[0];

        DY[0] = UR[1];
        DY[1] = UR[1];
        DY[2] = UR[1];

        DZ[0] = UR[2];
        DZ[1] = UR[2];
        DZ[2] = UR[2];

	_UR[0] = UR.x;
	_UR[1] = UR.y;
	_UR[2] = UR.z;
	
	_VR[0] = VR.x;
	_VR[1] = VR.y;
	_VR[2] = VR.z;
	
	//_U1[0] = U1.x;
	_U1[1] = U1.y;
	_U1[2] = U1.z;
	
	_V1[0] = V1.x;
	_V1[1] = V1.y;
	_V1[2] = V1.z;
	
	//_U2[0] = U2.x;
	_U2[1] = U2.y;
	_U2[2] = U2.z;
	
	_V2[0] = V2.x;
	_V2[1] = V2.y;
	_V2[2] = V2.z;
	
	//_U3[0] = U3.x;
	_U3[1] = U3.y;
	_U3[2] = U3.z;
	
	_V3[0] = V3.x;
	_V3[1] = V3.y;
	_V3[2] = V3.z;
	
        //__m128 __bar, __UR, __VR,
        //			  __U1, __V1,
        //			  __U2, __V2,
        //			  __U3, __V3;

        __m128 _AX, _AY, _AZ,
                                   _BX, _BY, _BZ,
                                   _CX, _CY, _CZ,
                                   _DX, _DY, _DZ;


        _AX = _mm_loadu_ps(AX);
        _AY = _mm_loadu_ps(AY);
        _AZ = _mm_loadu_ps(AZ);
        _BX = _mm_loadu_ps(BX);
        _BY = _mm_loadu_ps(BY);
        _BZ = _mm_loadu_ps(BZ);
        _CX = _mm_loadu_ps(CX);
        _CY = _mm_loadu_ps(CY);
        _CZ = _mm_loadu_ps(CZ);
        _DX = _mm_loadu_ps(DX);
        _DY = _mm_loadu_ps(DY);
        _DZ = _mm_loadu_ps(DZ);

				   
        /*__UR = _mm_loadu_ps(_UR);
	__VR = _mm_loadu_ps(_VR);
	__U1 = _mm_loadu_ps(_U1);
	__V1 = _mm_loadu_ps(_V1);
	__U2 = _mm_loadu_ps(_U2);
	__V2 = _mm_loadu_ps(_V2);
	__U3 = _mm_loadu_ps(_U3);
	__V3 = _mm_loadu_ps(_V3);
	
	__V1 = _mm_mul_ps(__V1,__UR);
	__U1 = _mm_mul_ps(__U1,__VR);
	__U1 = _mm_add_ps(__U1,__V1);
        */
	
	if(bar[2] > 0)
	{
		
		if(bar[0] > 0)
		{
			
			if( bar[1] > 0)
			{
				ti3 = m_mesh->nIndices()[m_index];
				const Vector3 & n0 = m_mesh->normals()[ti3.x]; //vertex a of triangle
				const Vector3 & n1 = m_mesh->normals()[ti3.y]; //vertex b of triangle
				const Vector3 & n2 = m_mesh->normals()[ti3.z]; //vertex c of triangle
				
				if(bar[0] + bar[1] + bar[2] >0)
				{
					bar /= bar[0] + bar[1] + bar[2];
					result.P = bar[0] * v0 + bar[1] * v1 + bar[2] * v2;
					result.t = (result.P-r.o).length();
					result.N = bar[0]*n0 + bar[1]*n1 + bar[2]*n2;
					result.N = result.N.normalized();
					result.material = m_material;
					return true;
					}
			}
		}
	}

	//free(res0);
	//free(res1);
	//free(res2);
	return false;
}
